#include "control/move_control/cfc.h"
#include "control/control_params.h"

using namespace control;

void CFCPathFollowing::Init(ros::NodeHandle n)
{
    is_in_dec_stage_ = false;
    running_type_ = RunningType::HEADFOLLOWING;
    stop_distance_threshold_.SetDownThreshold(g_pose_control_accuracy);
    stop_distance_threshold_.SetUpThreshold(g_pose_control_accuracy * 3.0f);

    ptr_speed_smooth_ = std::make_shared<common_lib::SpeedSmoothWithCloseConstAcc>(0.05);
    ptr_anglespeed_smooth_ = std::make_shared<common_lib::SpeedSmoothWithCloseConstAcc>(0.05);

    printf("g_cfc_max_speed_toward: %f\n", g_cfc_max_speed_toward);
}

void CFCPathFollowing::VehicleController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit)
{
    if (trajectory.points.empty())
    {
        speed = 0.0f;
        angle_speed = 0.0f;
        speed_toward = amr_state.actuator.speed_toward;
        return;
    }

    int fsm_switch_count_limit = 5;
    int fsm_head_angle_limit = 1;

    float speed_steer = 0;
    float angle_speed_steer = 0;
    float speed_toward_steer = amr_state.actuator.speed_toward;
    ref_point_ = trajectory.points[0];
    switch (running_type_)
    {
    case RunningType::HEADFOLLOWING:
        if (IsFollowingToRotating(trajectory, amr_state, ref_point_))
        {
            running_type_ = RunningType::ROTATING;
            speed = 0.0f;
            angle_speed = 0.0f;
            speed_toward = speed_toward_;
        }
        else if (IsFollowingToSteerturning(trajectory, amr_state, ref_point_, speed_toward))
        {
            running_type_ = RunningType::STEERTURNING;
            speed = 0.0f;
            angle_speed = 0;
            speed_toward = speed_toward_;
        }
        else
        {
            HeadFollowingController(trajectory, amr_state, speed, angle_speed, speed_toward, max_speed_limit, max_angle_speed_limit);
        }
        break;
    case RunningType::ROTATING:
        if (amr_state.control_state.cur_point_id == "")
        {
            for (int index = 0; index < trajectory.points.size(); index++)
            {
                if (index > 3) // TODO
                {
                    ref_point_ = trajectory.points[index - 1];
                    break;
                }
                float index_angle = trajectory.points[index].amr_angle;
                float dangle = fabs(com_.AngleConstraintToPI(index_angle - tf::getYaw(amr_state.amr_local_.pose.orientation)));

                if (dangle > g_online_angle)
                {
                    ref_point_ = trajectory.points[index];
                    break;
                }
            }
        }
        else
        {
            for (auto rit = trajectory.points.rbegin(); rit != trajectory.points.rend(); rit++)
            {
                if (amr_state.control_state.cur_point_id == rit->waypoint_id)
                {
                    ref_point_ = *(rit);
                    break;
                }
            }
        }

        if (IsRotatingToSteerTurning(ref_point_, amr_state))
        {
            running_type_ = RunningType::STEERTURNING;
            speed = 0.0f;
            angle_speed = 0;
            speed_toward = speed_toward_;
        }
        else
        {
            RotatingController(ref_point_, amr_state, speed, angle_speed, speed_toward, max_angle_speed_limit);
        }

        break;

    case RunningType::STEERTURNING:
        if (IsSteerturningToFollowing(ref_point_, amr_state, speed_toward))
        {
            running_type_ = RunningType::HEADFOLLOWING;
        }
        else
        {
            SteerTurningController(ref_point_, amr_state, speed, angle_speed, speed_toward);
        }
        break;
    default:
        break;
    }

    amr_state.running_type = running_type_;
    speed_ = speed;
    angle_speed_ = angle_speed;
    speed_toward_ = speed_toward;

    printf("CFCPathFollowing() --> running_type_: %d, path size: %d, v: %f, w: %f, toward: %f, target: %f %f %f, local: %f %f %f\n",
           running_type_, (int)trajectory.points.size(), speed, angle_speed, speed_toward,
           trajectory.points.back().pose.position.x, trajectory.points.back().pose.position.y, tf::getYaw(trajectory.points.back().pose.orientation),
           amr_state.amr_local_.pose.position.x, amr_state.amr_local_.pose.position.y, tf::getYaw(amr_state.amr_local_.pose.orientation));
}

void CFCPathFollowing::HeadFollowingController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit)
{
    float remain_dis = TargetLongDistance(amr_state, trajectory.points.back(), com_.AngleConstraintToPI(tf::getYaw(trajectory.points[0].pose.orientation)));
    amr_trajectory_msgs::point_info point = trajectory.points[0];
    float planning_speed = point.target_speed;

    float pose_control_speed = fabs(planning_speed);
    if (trajectory.points.back().distance < g_slow_down_distance)
    {
        pose_control_speed = (pose_control_speed > g_slow_down_speed) ? g_slow_down_speed : pose_control_speed;
    }

    if (trajectory.points.back().distance < g_position_control_distance)
    {
        // TODO
        if (trajectory.points.back().distance > g_creep_distance)
        {
            float speed_temp = g_position_control_kp * (trajectory.points.back().distance - g_creep_distance) + g_min_move_speed;
            if (pose_control_speed > speed_temp)
            {
                pose_control_speed = speed_temp;
            }
        }
        else
        {
            pose_control_speed = g_min_move_speed;
        }
    }

    if (pose_control_speed < g_min_move_speed)
    {
        pose_control_speed = g_min_move_speed;
    }
    
    planning_speed = ((point.move_direction=="forward") - (point.move_direction=="backward")) * pose_control_speed;
    
    if (stop_distance_threshold_.GetState(remain_dis) == common_lib::DoubleThreshold_e::DOEN_AREA && trajectory.points.size() < 3)
    {
        planning_speed = 0.0f;
    }
    distance_dec_ = trajectory.points.back().distance;

    LineSpeedLimit(amr_state, planning_speed, max_speed_limit);
    speed = planning_speed;

    if (fabs(planning_speed) < g_min_move_speed)
    {
        angle_speed = 0.0f;
        speed_toward = speed_toward_;
    }
    else
    {
        float lat_error = com_.Point2LineDistanceWithSign(amr_state.amr_local_.pose.position.x, amr_state.amr_local_.pose.position.y,
                                                          point.pose.position.x, point.pose.position.y, tf::getYaw(point.pose.orientation));
        float dir, dir_I;

        dir = g_lat_error_weight * lat_error;                                 // TODO
        dir = com_.ValueConstraintMinToMax(dir, -g_cfc_max_adjust_speed_toward, g_cfc_max_adjust_speed_toward); // TODO

        speed = planning_speed / cos(dir);

        float heading_error = com_.AngleConstraintToPI(point.amr_angle - tf::getYaw(amr_state.amr_local_.pose.orientation));
        if (point.angle_type == "turn" && point.curvature != 0)
        {
            angle_speed = com_.ValueConstraintMinToMax(g_heading_error_weight * heading_error + planning_speed * point.curvature, -M_PI_2 / 6.0f, M_PI_2 / 6.0f);
        }
        else
        {
            angle_speed = heading_error * com_.SectionLinearFcn(g_cfc_heading_error_weight_map_, fabs(speed));
        }
        float min_radius = (fabs(point.curvature) > FLT_EPSILON && point.angle_type == "turn") ? g_turn_min_radius : g_headfollowing_min_radius;
        if (fabs(angle_speed) > fabs(speed) / min_radius)
        {
            angle_speed = fabs(speed) / min_radius * (angle_speed / fabs(angle_speed));
        }

        float heading = (speed > 0) ? 0.0f : M_PI;
        speed_toward = com_.AngleConstraintToPI(dir + tf::getYaw(point.pose.orientation) + heading);
        speed_toward = com_.AngleConstraintToPI(speed_toward - tf::getYaw(amr_state.amr_local_.pose.orientation));
        speed_toward = com_.ValueConstraintMinToMax(speed_toward, -g_cfc_max_speed_toward, g_cfc_max_speed_toward);

        amr_state.lat_error = lat_error;
        amr_state.heading_error = heading_error;
    }
}

void CFCPathFollowing::RotatingController(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_angle_speed_limit)
{
    speed_toward = 0.0f;
    speed = 0.0f;

    float heading_error = point.amr_angle - tf::getYaw(amr_state.amr_local_.pose.orientation);
    angle_speed = com_.ValueConstraintMinToMax(g_cfc_rotating_kp * heading_error,
                                               -g_max_rotating_angle_speed, g_max_rotating_angle_speed);

    angle_speed = com_.ValueConstraintMinToMax(angle_speed, -fabs(max_angle_speed_limit), fabs(max_angle_speed_limit));
    float min_angle_speed = g_min_rotating_angle_speed;

    if (FLT_EPSILON < fabs(angle_speed) && g_min_move_speed > fabs(angle_speed) && fabs(amr_state.actuator.angle_speed) < 0.005f)
    {
        int sign = angle_speed > 0 ? 1 : -1;
        angle_speed = sign * min_angle_speed;
    }

    // AngleSpeed Smooth
    float target_angle_speed = angle_speed;
    float angle_max_acc = g_max_angle_speed_acc;
    float angle_max_dcc = g_max_angle_speed_dcc;
    float angle_speed_state = (fabs(amr_state.actuator.angle_speed) < min_angle_speed * 0.6f) ? 0.0f : amr_state.actuator.angle_speed;
    angle_speed = ptr_anglespeed_smooth_->VelocityConfig(target_angle_speed,
                                                         angle_speed_state,
                                                         angle_max_acc,
                                                         angle_max_dcc);

    angle_speed = ptr_anglespeed_smooth_->VelocityConfigToEnd(fabs(heading_error), angle_speed_state, angle_max_acc, angle_max_dcc);
    angle_speed = (heading_error>0.0f - heading_error<0.0f) * angle_speed;
    amr_state.heading_error = heading_error;
}

void CFCPathFollowing::SteerTurningController(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward)
{
    float heading = (point.move_direction == "forward") ? 0.0f : M_PI;
    speed_toward = 0.0f;
    speed = 0.0f;
    angle_speed = 0.0f;
    if (point.angle_type == "shift")
    {
        speed_toward = com_.AngleConstraintToPI(tf::getYaw(point.pose.orientation) + heading - tf::getYaw(amr_state.amr_local_.pose.orientation));
    }
}

bool CFCPathFollowing::IsFollowingToRotating(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, amr_trajectory_msgs::point_info &point)
{
#if 0
    for (int i = 0; i < trajectory.points.size(); i++)
    {
        float dtheta = fabs(com_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation) - trajectory.points[i].amr_angle));
        if (amr_state.control_state.cur_point_id == trajectory.points[i].waypoint_id && dtheta > g_f2r_pose_thea_path_angle_diff_thresh)
        {
            if (i < 3)
                return true;
        }
    }
#else
    float dtheta = fabs(com_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation) - trajectory.points[0].amr_angle));
#endif
    return (dtheta > g_f2r_pose_thea_path_angle_diff_thresh);
}

bool CFCPathFollowing::IsFollowingToSteerturning(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, amr_trajectory_msgs::point_info &point, float speed_toward)
{
    int index = 0;
    auto rit = trajectory.points.rbegin();
    float heading = (point.move_direction == "forward") ? 0.0f : M_PI;
    float local_angle = com_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation));
    float tmp_toward = com_.AngleConstraintToPI(tf::getYaw(point.pose.orientation) + amr_state.actuator.speed_toward + heading - local_angle);

    bool need_steer_local = fabs(tmp_toward) > g_f2s_line_theta_steer_angle_thresh;
    bool need_steer_actua = fabs(com_.AngleConstraintToPI(amr_state.actuator.speed_toward - speed_toward)) > g_f2s_actuator_steer_speed_toward_threshold;

    if ((need_steer_local || need_steer_actua) &&
        fabs(com_.AngleConstraintToPI(local_angle - trajectory.points[index].amr_angle)) < g_f2s_cur_angle_target_angle_threshold &&
        fabs(amr_state.actuator.walking_speed) < g_zero_speed &&
        (trajectory.points[index].curvature == 0 || trajectory.points[index].angle_type == "shift"))
    {
        point = trajectory.points[index];
        return true;
    }
    return false;
}
bool CFCPathFollowing::IsRotatingToFollowing(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state)
{
    if (fabs(com_.AngleConstraintToPI((trajectory.points[0].amr_angle - tf::getYaw(amr_state.amr_local_.pose.orientation)))) < g_r2f_heading_error)
    {
        return true;
    }
    return false;
}
bool CFCPathFollowing::IsRotatingToSteerTurning(amr_trajectory_msgs::point_info &point, AMRState &amr_state)
{
    if (fabs(com_.AngleConstraintToPI((point.amr_angle - tf::getYaw(amr_state.amr_local_.pose.orientation)))) < g_r2f_heading_error)
    {
        return true;
    }
    return false;
}
bool CFCPathFollowing::IsSteerturningToFollowing(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float speed_toward)
{
    float heading = (point.move_direction == "forward") ? 0.0f : M_PI;
    float local_angle = com_.AngleConstraintToPI(tf::getYaw(amr_state.amr_local_.pose.orientation));
    bool b_check_speed_toward_steer_angle = fabs(com_.AngleConstraintToPI(speed_toward-amr_state.actuator.speed_toward)) < g_s2f_steer_angle_speed_toward_diff_threshold;

    bool b_check_theta_steer_angle = fabs(com_.AngleConstraintToPI(amr_state.actuator.speed_toward+tf::getYaw(point.pose.orientation)+heading-local_angle))
                                     < g_s2f_global_steer_angle_diff_threshold;

    bool b_check_toward_theta = fabs(com_.AngleConstraintToPI(speed_toward+tf::getYaw(point.pose.orientation)+heading-local_angle))
                                < g_s2f_speed_toward_line_angle_diff_threshold;

    if ((b_check_toward_theta && b_check_speed_toward_steer_angle) || b_check_theta_steer_angle)
    {
        return true;
    }
    return false;
}
void CFCPathFollowing::LineSpeedLimit(AMRState &amr_state, float &speed, float max_speed_limit) 
{
    float target_speed = speed;
    if (fabs(target_speed) > max_speed_limit)
    {
        target_speed = ((target_speed>FLT_EPSILON) - (target_speed<-FLT_EPSILON)) * max_speed_limit;
    }

    // StartSpeed
    if (fabs(amr_state.actuator.walking_speed) < g_zero_speed)
    {
        if ( g_zero_speed < fabs(target_speed) && g_min_move_speed > fabs(target_speed))
        {
            int sign = target_speed > 0 ? 1: -1;
            target_speed = sign * g_min_move_speed;
        }
    }
    // ROS_INFO("LineSpeedLimit(): v: %f", amr_state.actuator.walking_speed);
    float walking_speed = (fabs(amr_state.actuator.walking_speed)<g_min_move_speed*0.6f)?0.0f:amr_state.actuator.walking_speed;
    float speed_max_acc = g_max_acc;
    float speed_max_dcc = g_max_dcc;

    speed = ptr_speed_smooth_->VelocityConfig(target_speed, walking_speed, speed_max_acc, speed_max_dcc);
    
    float calc_dec_velocity = sqrt(fabs(2 * speed_max_dcc * distance_dec_)); // v^2 = 2as

    // 计算当前位置的理论速度与实际速度差值
    float vel_error    = std::max(fabs(amr_state.actuator.walking_speed) - calc_dec_velocity, 0.0f);
    float vel_error_k  = g_dec_vel_error_k;
    float time_delay   = g_dec_response_delay;
    float pre_distance = distance_dec_ - fabs(amr_state.actuator.walking_speed) * (time_delay + vel_error / speed_max_dcc * vel_error_k);
    float dec_speed = ptr_speed_smooth_->VelocityConfigToEnd(pre_distance, walking_speed, speed_max_acc, speed_max_dcc);
    
    // ROS_INFO("Planning_speed: line: %d, v: %f, dec_speed: %f, distance: %f %f", __LINE__, speed, dec_speed, distance_dec_, pre_distance);
    if (fabs(dec_speed) < g_min_move_speed && distance_dec_ > 0)
    {
        dec_speed = g_min_move_speed;
    }
    
    if (fabs(dec_speed) < fabs(speed))
    {
        int sign = speed >= 0 ? 1 : -1;
        speed = ((speed>FLT_EPSILON) - (speed<-FLT_EPSILON)) * dec_speed;
        // ptr_speed_smooth_->SyncSpeed(speed, g_speed_plan_type);
    }
}

float CFCPathFollowing::TargetLongDistance(AMRState &amr_state, amr_trajectory_msgs::point_info target_point, float line_theta)
{
    float dis;
    float heading;
    if (target_point.target_speed > 0)
    {
        heading = 0;
    }
    else
    {
        heading = M_PI;
    }
    float theta = tf::getYaw(amr_state.amr_local_.pose.orientation);
    float global_steer_angle = com_.AngleConstraintToPI(theta + heading);
    global_steer_angle = line_theta;
    dis = (target_point.pose.position.x - amr_state.amr_local_.pose.position.x) * cos(global_steer_angle) + (target_point.pose.position.y - 
           amr_state.amr_local_.pose.position.y) * sin(global_steer_angle); // + 0.007;
    if (dis < 0)
    {
        return 0.0f;
    }
    else
    {
        return dis;
    }
}